#include "ros_bridge.h"

#include <cinttypes>

#include <tbox/base/log.h>
#include <tbox/base/json.hpp>
#include <tbox/base/recorder.h>

extern rclcpp::Executor::WeakPtr g_ros_exec_wptr;

namespace your {

RosBridge::RosBridge(tbox::main::Context &ctx, Parent &parent) :
  tbox::main::Module("ros", ctx),
  parent_(parent),
  worker_(ctx.loop()),
  node_(std::make_shared<rclcpp::Node>("tbox"))
{
  using namespace std::placeholders;

  ready_timer_ = node_->create_wall_timer(std::chrono::milliseconds(1), std::bind(&RosBridge::onTimerTick, this));
  pub_string_ = node_->create_publisher<std_msgs::msg::String>("topic_text", 10);
  sub_bool_ = node_->create_subscription<std_msgs::msg::Bool>("topic_bool", 1, std::bind(&RosBridge::onTopicSubBool, this, _1));
  add_two_ints_client_ = node_->create_client<example_interfaces::srv::AddTwoInts>("add_two_ints");
  add_two_ints_service_ = node_->create_service<example_interfaces::srv::AddTwoInts>("add_two_ints", std::bind(&RosBridge::onServiceAddTwoInts, this, _1, _2, _3));
}

RosBridge::~RosBridge() { }

void RosBridge::onFillDefaultConfig(tbox::Json &js_this) { }

bool RosBridge::onInit(const tbox::Json &js_this) { return true; }

bool RosBridge::onStart() {
  auto exec = g_ros_exec_wptr.lock();
  if (exec) {
    exec->add_node(node_);
    return true;

  } else {
    LogErr("exec == nullptr");
    return false;
  }
}

void RosBridge::onStop() { }

void RosBridge::toJson(tbox::Json &js) const { }

void RosBridge::pubText(const std::string &text) {
  RECORD_SCOPE();
  std_msgs::msg::String msg;
  msg.data = text;

  //! 委托给子线程去发布，防止阻塞主线程
  worker_.execute(
    [this, msg] () {
      RECORD_SCOPE();
      pub_string_->publish(msg);
    }
  );
}

void RosBridge::addTwoInt(int a, int b, AddTwoIntCallback &&cb) {
  RECORD_SCOPE();
  auto req = std::make_shared<example_interfaces::srv::AddTwoInts::Request>();
  req->a = a;
  req->b = b;
  add_two_ints_client_->async_send_request(req,
    [cb = std::move(cb)](rclcpp::Client<example_interfaces::srv::AddTwoInts>::SharedFuture future) {
      RECORD_SCOPE();
      auto result = future.get();
      if (result) {
        cb(true, result->sum);
      } else {
        cb(false, 0);
      }
    }
  );
}

void RosBridge::onTimerTick() {
  RECORD_SCOPE();
  ready_timer_->cancel();
  //! 转交给主线程处理，防止子线程操作外部资源
  ctx().loop()->runInLoop(
    [this] () {
      RECORD_SCOPE();
      parent_.on_Ros_ready();
    }
  );
}

void RosBridge::onTopicSubBool(const std_msgs::msg::Bool::SharedPtr msg) {
  RECORD_SCOPE();
  //! 转交给主线程处理，防止子线程操作外部资源
  ctx().loop()->runInLoop(
    [this, msg] () {
      RECORD_SCOPE();
      parent_.on_Ros_gotBool(msg->data);
    }
  );
}

//! 这里演示异步处理的情况
//! 如何将子线程的同步请求转化成主线程的异步处理
void RosBridge::onServiceAddTwoInts(const std::shared_ptr<rmw_request_id_t> request_header,
                                    const std::shared_ptr<example_interfaces::srv::AddTwoInts::Request> req,
                                    const std::shared_ptr<example_interfaces::srv::AddTwoInts::Response> rsp) {
  LogInfo("request: %" PRId64 " + %" PRId64, req->a, req->b);
  std::promise<int> p;
  auto f = p.get_future();

  //! 转交由主线程去进行异步处理
  ctx().loop()->runInLoop([this, req, rsp, &p] {
    parent_.on_Ros_addTwoInts(req->a, req->b, p);
  });

  f.wait();
  rsp->sum = f.get();
  LogInfo("respond: %" PRId64, rsp->sum);
}

//! 注：RECORD_SCOPE() 不是必须的，它只是用于记录函数调用的性能数据，便于调试和优化。
//! 你可以根据需要选择是否使用它。
//! 如果不需要性能记录，可以将其删除或注释掉。

}
